/*
 * protocol.c - 协议实现
 *
 * Author: xiaobo@YGL (xiaobocwb@gmail.com)
 * Version: 1.0.0
 * Date: 2024-01
 */

#include <stdio.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include "protocol.h"

 // 帧格式常量定义
#define FRAME_HEAD_MAGIC    0xAA55  // 帧头魔数
#define FRAME_HEAD_SIZE     2       // 帧头长度
#define FRAME_LENGTH_SIZE   2       // 长度字段长度
#define FRAME_MSGID_SIZE    1       // 消息ID字段长度
#define FRAME_SEQ_SIZE      1       // 序列号字段长度
#define FRAME_CRC_SIZE      2       // CRC长度

#define MAX_PAYLOAD_LEN     1024    // 负载最大长度
#define MAX_SEQ             0x7F    // 序列号最大值
#define FRAME_BASE_SIZE     (FRAME_HEAD_SIZE + FRAME_LENGTH_SIZE + FRAME_MSGID_SIZE + FRAME_SEQ_SIZE + FRAME_CRC_SIZE)
#define MAX_FRAME_SIZE      (FRAME_BASE_SIZE + MAX_PAYLOAD_LEN)

#define FRAME_MSGID_ACK     0xFF    // ACK响应ID

#define CRC16_POLY 0x8005
#define CRC16_INIT 0xFFFF


// 解析状态
typedef enum {
    PARSE_STATE_HEAD1,      // 帧头第一个字节
    PARSE_STATE_HEAD2,      // 帧头第二个字节
    PARSE_STATE_LENGTH_L,   // 长度低字节
    PARSE_STATE_LENGTH_H,   // 长度高字节
    PARSE_STATE_MSGID,      // 消息ID字节
    PARSE_STATE_SEQ,        // 序列号字节
    PARSE_STATE_DATA,       // 数据
    PARSE_STATE_CRC_L,      // CRC低字节
    PARSE_STATE_CRC_H       // CRC高字节
} parse_state_t;

// 解析上下文
typedef struct {
    parse_state_t state;    // 当前解析状态
    uint16_t length;        // 帧长度
    uint8_t msgid;          // 消息ID
    uint8_t seq;            // 序列号
    uint8_t data[MAX_FRAME_SIZE];  // 数据缓冲区
    uint16_t data_index;    // 当前数据索引
    uint16_t crc;           // CRC值
} parse_context_t;

// 等待ACK的事件结构
typedef struct {
    void* sem;
} ack_event_t;

// 协议上下文结构
struct protocol_ctx {
    protocol_hal_t hal;                 // 硬件抽象层
    parse_context_t parse_ctx;          // 解析上下文
    msg_callback_t subscribers[FRAME_MSGID_SIZE * UINT8_MAX];    // 订阅者回调函数表
    uint8_t next_seq;                   // 下一个序列号

    // 线程同步相关
    void* seq_mutex;                    // 序列号分配互斥锁
    void* sub_mutex;                    // 订阅者表互斥锁
    ack_event_t* ack_events[MAX_SEQ * FRAME_SEQ_SIZE];       // 等待ACK的事件表
    void* ack_mutex;                    // ACK事件表互斥锁
};

static uint16_t crc16_calc(const uint8_t* data, size_t length)
{
    uint16_t crc = CRC16_INIT;

    for (size_t i = 0; i < length; i++) {
        crc ^= (uint16_t)data[i] << 8;
        for (int j = 0; j < 8; j++) {
            if (crc & 0x8000) {
                crc = (crc << 1) ^ CRC16_POLY;
            }
            else {
                crc <<= 1;
            }
        }
    }

    return crc;
}

// 分配序列号
static uint8_t alloc_seq(struct protocol_ctx* ctx)
{
    uint8_t seq;
    ctx->hal.mutex_lock(ctx->seq_mutex);
    ctx->next_seq = (ctx->next_seq + 1) % MAX_SEQ;
    seq = ctx->next_seq;
    ctx->hal.mutex_unlock(ctx->seq_mutex);
    return seq;
}

// 发送帧
static bool send_frame(struct protocol_ctx* ctx, uint8_t msgid, uint8_t seq, const uint8_t* payload, size_t payload_len)
{
    uint8_t* frame = (uint8_t*)malloc(FRAME_BASE_SIZE + payload_len);  // 帧缓冲区
    if (!frame) {
        return false;
    }
    uint16_t frame_len = 0;
    uint16_t crc;

    // 构建帧
    frame[frame_len++] = 0xAA;  // 帧头
    frame[frame_len++] = 0x55;

    uint16_t length = payload_len + FRAME_MSGID_SIZE + FRAME_SEQ_SIZE + FRAME_CRC_SIZE; // + 消息ID + 序列号 + CRC size
    frame[frame_len++] = length & 0xFF;
    frame[frame_len++] = length >> 8;

    frame[frame_len++] = msgid;
    frame[frame_len++] = seq;

    if (payload && payload_len > 0) {
        memcpy(&frame[frame_len], payload, payload_len);
        frame_len += payload_len;
    }

    // 计算并添加CRC
    crc = crc16_calc(frame + FRAME_HEAD_SIZE, frame_len - FRAME_HEAD_SIZE);  // 从长度字段开始计算
    frame[frame_len++] = crc & 0xFF;
    frame[frame_len++] = crc >> 8;

    // 发送帧
    size_t ret = ctx->hal.send(ctx->hal.dev, frame, frame_len);
    free(frame);
    return ret == frame_len;
}

// 处理完整的帧
static int handle_frame(protocol_t proto)
{
    struct protocol_ctx* ctx = (struct protocol_ctx*)proto;

    if (ctx->parse_ctx.msgid == FRAME_MSGID_ACK) {
        // 处理ACK
        proto->hal.mutex_lock(ctx->ack_mutex);

        uint8_t seq = ctx->parse_ctx.data[FRAME_LENGTH_SIZE + FRAME_MSGID_SIZE + FRAME_SEQ_SIZE];
        ack_event_t* event = ctx->ack_events[seq & 0x7F];

        if (event && event->sem) {
            proto->hal.sem_post(event->sem);
        }

        proto->hal.mutex_unlock(ctx->ack_mutex);
    }
    else {
        ctx->hal.mutex_lock(ctx->sub_mutex);
        msg_callback_t callback = ctx->subscribers[ctx->parse_ctx.msgid];
        if (callback) {
            callback(ctx->parse_ctx.msgid, &ctx->parse_ctx.data[FRAME_LENGTH_SIZE + FRAME_MSGID_SIZE + FRAME_SEQ_SIZE], ctx->parse_ctx.data_index - FRAME_LENGTH_SIZE - FRAME_MSGID_SIZE - FRAME_SEQ_SIZE);
            uint8_t seq = ctx->parse_ctx.data[FRAME_LENGTH_SIZE + FRAME_MSGID_SIZE];
            if (seq & 0x80) {
                // 发送ACK
                seq &= 0x7F;
                send_frame(ctx, FRAME_MSGID_ACK, alloc_seq(ctx), &seq, FRAME_SEQ_SIZE);
            }
        }
        ctx->hal.mutex_unlock(ctx->sub_mutex);
    }

    return 0;
}

static void protocol_input_byte(protocol_t proto, uint8_t byte)
{
    struct protocol_ctx* ctx = (struct protocol_ctx*)proto;
    parse_context_t* parse = &ctx->parse_ctx;

    switch (parse->state) {
    case PARSE_STATE_HEAD1:
        if (byte == (FRAME_HEAD_MAGIC >> 8)) {
            parse->state = PARSE_STATE_HEAD2;
        }
        break;

    case PARSE_STATE_HEAD2:
        if (byte == (FRAME_HEAD_MAGIC & 0xFF)) {
            parse->state = PARSE_STATE_LENGTH_L;
            parse->data_index = 0;
        }
        else {
            parse->state = PARSE_STATE_HEAD1;
        }
        break;

    case PARSE_STATE_LENGTH_L:
        parse->length = byte;
        parse->data[parse->data_index++] = byte;
        parse->state = PARSE_STATE_LENGTH_H;
        break;

    case PARSE_STATE_LENGTH_H:
        parse->length |= byte << 8;
        parse->data[parse->data_index++] = byte;

        int min_length = FRAME_SEQ_SIZE + FRAME_MSGID_SIZE + FRAME_CRC_SIZE;
        if (parse->length < min_length || parse->length > min_length + MAX_PAYLOAD_LEN) {
            parse->state = PARSE_STATE_HEAD1;
        }
        else {
            parse->state = PARSE_STATE_MSGID;
        }
        break;

    case PARSE_STATE_MSGID:
        parse->msgid = byte;
        parse->data[parse->data_index++] = byte;
        parse->length--;  // 减去消息ID字节
        parse->state = PARSE_STATE_SEQ;
        break;

    case PARSE_STATE_SEQ:
        parse->seq = byte;
        parse->data[parse->data_index++] = byte;
        parse->length--;  // 减去序列号字节
        parse->length -= FRAME_CRC_SIZE;  // 减去CRC字节
        if (parse->length > 0) {
            parse->state = PARSE_STATE_DATA;
        }
        else {
            parse->state = PARSE_STATE_CRC_L;
        }
        break;

    case PARSE_STATE_DATA:
        parse->data[parse->data_index++] = byte;
        if (--parse->length == 0) {
            parse->state = PARSE_STATE_CRC_L;
        }
        break;

    case PARSE_STATE_CRC_L:
        parse->crc = byte;
        parse->state = PARSE_STATE_CRC_H;
        break;

    case PARSE_STATE_CRC_H:
        parse->crc |= byte << 8;
        if (parse->crc == crc16_calc(parse->data, parse->data_index)) {
            handle_frame(proto);  // 处理完整帧
        }
        else {
            // 校验失败
            printf("[Protocol] CRC check failed, seq: %d, crc: %04X\n", parse->seq, parse->crc);
        }
        parse->state = PARSE_STATE_HEAD1;  // 重置状态
        break;

    default:
        parse->state = PARSE_STATE_HEAD1;
        break;
    }
}

// 创建协议实例
protocol_t protocol_create(const protocol_hal_t* hal)
{
    if (!hal || !hal->send
        || !hal->mutex_new || !hal->mutex_del || !hal->mutex_lock || !hal->mutex_unlock
        || !hal->sem_new || !hal->sem_del || !hal->sem_wait || !hal->sem_post) {
        return NULL;
    }

    struct protocol_ctx* ctx = (struct protocol_ctx*)malloc(sizeof(struct protocol_ctx));
    if (!ctx) {
        return NULL;
    }

    memset(ctx, 0, sizeof(struct protocol_ctx));
    memcpy(&ctx->hal, hal, sizeof(protocol_hal_t));
    ctx->next_seq = 0;

    // 初始化互斥锁
    ctx->seq_mutex = ctx->hal.mutex_new();
    ctx->ack_mutex = ctx->hal.mutex_new();
    ctx->sub_mutex = ctx->hal.mutex_new();

    if (!ctx->seq_mutex || !ctx->ack_mutex || !ctx->sub_mutex) {
        protocol_destroy(ctx);
        return NULL;
    }

    return (protocol_t)ctx;
}

// 销毁协议实例
void protocol_destroy(protocol_t proto)
{
    struct protocol_ctx* ctx = (struct protocol_ctx*)proto;
    if (!ctx) {
        return;
    }

    // 销毁互斥锁
    if (ctx->seq_mutex) {
        ctx->hal.mutex_del(ctx->seq_mutex);
    }
    if (ctx->ack_mutex) {
        ctx->hal.mutex_del(ctx->ack_mutex);
    }
    if (ctx->sub_mutex) {
        ctx->hal.mutex_del(ctx->sub_mutex);
    }

    free(ctx);
}

void* protocol_get_dev(protocol_t proto)
{
    struct protocol_ctx* ctx = (struct protocol_ctx*)proto;
    return ctx->hal.dev;
}

bool protocol_prase(protocol_t proto, uint8_t* buffer, size_t len)
{
    struct protocol_ctx* ctx = (struct protocol_ctx*)proto;
    if (!ctx || !buffer || len == 0) {
        return false;
    }

    for (size_t i = 0; i < len; i++) {
        protocol_input_byte(proto, buffer[i]);
    }
    return true;
}

// 发布消息
bool protocol_publish(protocol_t proto, uint8_t msgid, const uint8_t* payload, size_t payload_len, bool need_ack, uint32_t timeout)
{
    struct protocol_ctx* ctx = (struct protocol_ctx*)proto;
    if (!ctx || payload_len > MAX_PAYLOAD_LEN) {
        return false;
    }

    uint8_t seq;
    bool is_busy;

    for (int i = 0; i < MAX_SEQ; i++) {
        seq = alloc_seq(ctx);
        is_busy = ctx->ack_events[seq] ? true : false;
        if (!is_busy) {
            break;
        }
    }

    if (is_busy) {
        printf("[Protocol] not found free seq\n");
        return false;
    }

    ack_event_t event = { 0 };

    if (need_ack) {
        seq |= 0x80;
        // 创建ACK等待事件
        event.sem = ctx->hal.sem_new();

        // 注册事件
        ctx->hal.mutex_lock(ctx->ack_mutex);
        ctx->ack_events[seq & 0x7F] = &event;
        ctx->hal.mutex_unlock(ctx->ack_mutex);
    }

    if (!send_frame(ctx, msgid, seq, payload, payload_len)) {
        if (need_ack) {
            ctx->hal.mutex_lock(ctx->ack_mutex);
            ctx->ack_events[seq & 0x7F] = NULL;
            ctx->hal.mutex_unlock(ctx->ack_mutex);

            ctx->hal.sem_del(event.sem);
        }
        return false;
    }

    bool is_ack = true;

    if (need_ack) {
        // 等待ACK
        is_ack = ctx->hal.sem_wait(event.sem, timeout);

        ctx->hal.mutex_lock(ctx->ack_mutex);
        ctx->ack_events[seq & 0x7F] = NULL;
        ctx->hal.mutex_unlock(ctx->ack_mutex);
        ctx->hal.sem_del(event.sem);
    }

    return is_ack;
}

bool protocol_publish_try(protocol_t proto, uint8_t msgid, const uint8_t* payload, size_t payload_len, bool need_ack, uint32_t timeout, uint32_t try_times)
{
    if (proto == NULL) return false;

    try_times = try_times > 0 ? try_times : 1;
    for (uint32_t i = 0; i < try_times; i++) {
        if (protocol_publish(proto, msgid, payload, payload_len, need_ack, timeout)) {
            return true;
        }
    }
    return false;
}

// 订阅主题
bool protocol_subscribe(protocol_t proto, uint8_t msgid, msg_callback_t callback)
{
    struct protocol_ctx* ctx = (struct protocol_ctx*)proto;
    if (!ctx || !callback) {
        return false;
    }
    ctx->hal.mutex_lock(ctx->sub_mutex);
    ctx->subscribers[msgid] = callback;
    ctx->hal.mutex_unlock(ctx->sub_mutex);

    return true;
}

// 取消订阅主题
bool protocol_unsubscribe(protocol_t proto, uint8_t msgid)
{
    struct protocol_ctx* ctx = (struct protocol_ctx*)proto;
    if (!ctx) {
        return false;
    }

    ctx->hal.mutex_lock(ctx->sub_mutex);
    ctx->subscribers[msgid] = (msg_callback_t){ 0 };
    ctx->hal.mutex_unlock(ctx->sub_mutex);
    return true;
}
